#include <unistd.h>
#include <netdb.h> /* struct hostent */
#include <arpa/inet.h> /* inet_ntop */
#include <sys/types.h>
#include <ifaddrs.h>
#include <netinet/in.h>
#include "MqttPlannerServer.h"
#include "AiBox_LOG.h"
#include "cJSON.h"
#include "OMPLPlanner.h"
#include "ObstacleManage.h"
#define mqtt_port 51284
static const  std::string MQTT_SERVER_IP ("127.0.0.1");
static const  std::string MQTT_CLIENT_ID("MQTT_planner_server");
static const  std::string MQTT_ORIGIN_LOCATION_TOPIC("MQTT_leader_location");
static const  std::string MQTT_PLANNER_TOPIC_LOCATION("MQTT_planner_server_node_location");
static const  std::string MQTT_PLANNER_TOPIC_PLAN("MQTT_planner_server_node_plan_request");
static const  std::string MQTT_PLANNER_TOPIC_PATH("MQTT_planner_server_path/");
using namespace Planner;
std::shared_ptr<MqttPlannerServer> MqttPlannerServer::_singleton_instance=nullptr;
std::shared_ptr<MqttPlannerServer> 
MqttPlannerServer::getInstance()
{
    if(_singleton_instance.get()==nullptr){
        new MqttPlannerServer();
    }
    return _singleton_instance;
}
MqttPlannerServer::MqttPlannerServer()
{
    _singleton_instance=std::shared_ptr<MqttPlannerServer>(this);
}
void 
MqttPlannerServer::startMqtt()
{
	if(_mosq!=NULL){
		mqttDisconnect();
	}
	if(!init()){
		AIBOX_LOGE("Init mqtt err.");
	}
}
bool 
MqttPlannerServer::init()
{
	if(!getLocalIP()){
        AIBOX_LOGE("get local ip err.");
        return false;
    }
    //step2: init the mqtt client
    mosquitto_lib_init();
    _mosq=mosquitto_new(MQTT_CLIENT_ID.c_str(), true, nullptr);
    if(!_mosq){
        AIBOX_LOGE("Error:Creat a new mosquitto err.");
        return false;
    }
    mosquitto_subscribe_callback_set(_mosq, &MqttPlannerServer::subscribeCallback);
	mosquitto_connect_callback_set(_mosq, &MqttPlannerServer::connectCallback);
	mosquitto_message_callback_set(_mosq, &MqttPlannerServer::messageCallback);
    mosquitto_disconnect_callback_set(_mosq,&MqttPlannerServer::disconnectCallback);
    //set the mqtt version
    int mqtt_pro_ver=MQTT_PROTOCOL_V311;
    mosquitto_opts_set(_mosq, MOSQ_OPT_PROTOCOL_VERSION,&mqtt_pro_ver);
	//set the reconnect delay 2s, max is 20s
	mosquitto_reconnect_delay_set(_mosq,2,20,false);
    if((!_mqtt_username.empty() ||!_mqtt_password.empty()) && mosquitto_username_pw_set(_mosq, _mqtt_username.c_str(), _mqtt_password.c_str())){
		AIBOX_LOGE("Error: Problem setting username and/or password.");
		mosquitto_lib_cleanup();
		return false;
	}
    //connect 
    int rc = mosquitto_connect(_mosq, MQTT_SERVER_IP.c_str(), mqtt_port, 30);
    if(rc!=MOSQ_ERR_SUCCESS){
        AIBOX_LOGE("Mqtt connect bind err,code: ",rc);
        mosquitto_lib_cleanup();
        return false;
    }
    if(mosquitto_loop_start(_mosq)!=MOSQ_ERR_SUCCESS){
        AIBOX_LOGE("Mqtt start err");
        mosquitto_destroy(_mosq);
        mosquitto_lib_cleanup();
        return false;
    }
    return true;
}
void 
MqttPlannerServer::subscribeCallback(struct mosquitto* mosq, void* obj, int msg_id, int qos_count, const int* granted_qos)
{
	AIBOX_LOGI("Mqtt subscribe success.");
}
void 
MqttPlannerServer::connectCallback(struct mosquitto* mosq, void* obj, int response)
{
	if(response==0){
        AIBOX_LOGI("Mqtt server connect success.");
        //sub the topic
        int rc=mosquitto_subscribe(mosq,NULL,MQTT_PLANNER_TOPIC_LOCATION.c_str(),2);
        if(rc!=MOSQ_ERR_SUCCESS){
            AIBOX_LOGE("Mqtt sub topic",MQTT_PLANNER_TOPIC_LOCATION," err.");
        }
        rc=mosquitto_subscribe(mosq,NULL,MQTT_PLANNER_TOPIC_PLAN.c_str(),2);
        if(rc!=MOSQ_ERR_SUCCESS){
            AIBOX_LOGE("Mqtt sub topic",MQTT_PLANNER_TOPIC_PLAN," err.");
        }
        rc =mosquitto_subscribe(mosq,NULL,MQTT_ORIGIN_LOCATION_TOPIC.c_str(),2);
        if(rc!=MOSQ_ERR_SUCCESS){
            AIBOX_LOGE("Mqtt sub topic",MQTT_ORIGIN_LOCATION_TOPIC," err.");
        }
        _singleton_instance->_mqtt_is_connected=true;
    }else{
        AIBOX_LOGW("Mqtt connect refused,code: ",response);
    }
}
void 
MqttPlannerServer::messageCallback(struct mosquitto* mosq, void* obj, const struct mosquitto_message* msg)
{
    std::string payload_str=std::string((char*)msg->payload,msg->payloadlen);
    cJSON* payload_json=cJSON_Parse(payload_str.c_str());
    if(payload_json==NULL){
        AIBOX_LOGE("Parse payload err.");
        return;
    }
    cJSON* deviceid_json=cJSON_GetObjectItem(payload_json,"ID");
    if(deviceid_json==NULL){
        AIBOX_LOGE("Parse ID err.");
        cJSON_Delete(payload_json);
        return;
    }
    int vehicle_id=deviceid_json->valueint;
    if(MQTT_ORIGIN_LOCATION_TOPIC.compare(msg->topic)==0){ //handle the origin topic,this topic from the GCS
        //{"ID":xx,"OLAT":xxx,"OLON":xxx,"OALT":xxx}
        cJSON* device_lat_json=cJSON_GetObjectItem(payload_json,"OLAT");
        if(device_lat_json==NULL){
            AIBOX_LOGE("Parse LAT err.");
            cJSON_Delete(payload_json);
            return;
        }
        int32_t lat=device_lat_json->valueint;
        //set the origin lat
        LocationInfo::setOriginLat(lat);
        cJSON* device_lon_json=cJSON_GetObjectItem(payload_json,"OLON");
        if(device_lon_json==NULL){
            AIBOX_LOGE("Parse LON err.");
            cJSON_Delete(payload_json);
            return;
        }
        int32_t lon=device_lon_json->valueint;
        //set the origin lon
        LocationInfo::setOriginLon(lon);
    }else if(MQTT_PLANNER_TOPIC_LOCATION.compare(msg->topic)==0){
        //alloc the node location info
        //{"ID":xx,"LAT":xxx,"LON":xxx,"ALT":xxx}
        cJSON* device_lat_json=cJSON_GetObjectItem(payload_json,"LAT");
        if(device_lat_json==NULL){
            AIBOX_LOGE("Parse LAT err.");
            cJSON_Delete(payload_json);
            return;
        }
        int32_t lat=device_lat_json->valueint;
        cJSON* device_lon_json=cJSON_GetObjectItem(payload_json,"LON");
        if(device_lon_json==NULL){
            AIBOX_LOGE("Parse LON err.");
            cJSON_Delete(payload_json);
            return;
        }
        int32_t lon=device_lon_json->valueint;
        //check the origin point valid
        if(LocationInfo::getOriginLat()<0||LocationInfo::getOriginLon()<0){
            AIBOX_LOGW("Origin is not set.Topic:",MQTT_PLANNER_TOPIC_LOCATION);
            return;
        }
        ObstacleManage::getInstance()->addObstacle(vehicle_id,LocationInfo(lat,lon));
    }else if(MQTT_PLANNER_TOPIC_PLAN.compare(msg->topic)==0){
        //execute the plan requst
        //{"ID":xx,"SLAT":xxx,"SLON":xxx,"GLAT":xxx,"GLON":xxx}
        //start
        cJSON* device_slat_json=cJSON_GetObjectItem(payload_json,"SLAT");
        if(device_slat_json==NULL){
            AIBOX_LOGE("Parse SLAT err.");
            cJSON_Delete(payload_json);
            return;
        }
        int32_t start_lat=device_slat_json->valueint;
        cJSON* device_slon_json=cJSON_GetObjectItem(payload_json,"SLON");
        if(device_slon_json==NULL){
            AIBOX_LOGE("Parse SLON err.");
            cJSON_Delete(payload_json);
            return;
        }
        int32_t start_lon=device_slon_json->valueint;
        LocationInfo start_point(start_lat,start_lon);
        //global
        cJSON* device_glat_json=cJSON_GetObjectItem(payload_json,"GLAT");
        if(device_glat_json==NULL){
            AIBOX_LOGE("Parse GLAT err.");
            cJSON_Delete(payload_json);
            return;
        }
        int32_t target_lat=device_glat_json->valueint;
        cJSON* device_glon_json=cJSON_GetObjectItem(payload_json,"GLON");
        if(device_glon_json==NULL){
            AIBOX_LOGE("Parse GLON err.");
            cJSON_Delete(payload_json);
            return;
        }
        int32_t target_lon=device_glon_json->valueint;
        if(LocationInfo::getOriginLat()<0||LocationInfo::getOriginLon()<0){
            AIBOX_LOGW("Origin is not set.Topic",MQTT_PLANNER_TOPIC_PLAN);
            return;
        }
        LocationInfo target_point(target_lat,target_lon);
        ObstacleManage::getInstance()->addTarget(vehicle_id,target_point);
        //new a planner
        new OMPLPlanner(vehicle_id,start_point,target_point,ObstacleManage::getInstance()->getObstacleMap());
    }else{
        AIBOX_LOGW("Unkown Mqtt mesg topic");
    }
    return;	
}
void 
MqttPlannerServer::disconnectCallback(struct mosquitto *mosq, void *obj, int rc)
{
	if(rc!=0){
		//reconnect if sever lost
		mosquitto_reconnect_async(mosq);
	}
	_singleton_instance->_mqtt_is_connected=false;
    AIBOX_LOGI("Mqtt disconnected,code: ",rc);
}
void 
MqttPlannerServer::mqttDisconnect()
{
	if(_mosq){
		_mqtt_is_connected=false;
		mosquitto_loop_stop(_mosq,true);
		mosquitto_destroy(_mosq);
		mosquitto_lib_cleanup();
		_mosq=NULL;
	}
}
void 
MqttPlannerServer::pubPathMsg(int id,const std::string& msg_json)
{
    std::string topic=MQTT_PLANNER_TOPIC_PATH+std::to_string(id);
    if(_mqtt_is_connected&&_mosq!=NULL){
        int ret=mosquitto_publish(_mosq,NULL,topic.c_str(),msg_json.size()+1,(void*)msg_json.c_str(),2,false);
        if(ret!=MOSQ_ERR_SUCCESS){
            AIBOX_LOGE("Publish topic:",topic," err.");
        }
    }else{
        AIBOX_LOGE("Mqtt isn't connect.");
    }
}
bool 
MqttPlannerServer::getLocalIP()
{
    struct ifaddrs * ifAddrStruct=NULL;
    struct ifaddrs * ifa=NULL;
    void * tmpAddrPtr=NULL;
 
    getifaddrs(&ifAddrStruct);
    if (ifAddrStruct==NULL){
        return false;
    }
    for (ifa = ifAddrStruct; ifa != NULL; ifa = ifa->ifa_next)
	{
        if (!ifa->ifa_addr){
            continue;
        }
        if (ifa->ifa_addr->sa_family == AF_INET) // check it is IP4
		{
            // is a valid IP4 Address
            tmpAddrPtr=&((struct sockaddr_in *)ifa->ifa_addr)->sin_addr;
            char addressBuffer[INET_ADDRSTRLEN];
            inet_ntop(AF_INET, tmpAddrPtr, addressBuffer, INET_ADDRSTRLEN);
            _local_ip=addressBuffer;
            if(_local_ip.compare("127.0.0.1")!=0){
                return true;   
            }
        }
    }
	freeifaddrs(ifAddrStruct);
    return false;
}
